import threading
import cv2
import numpy as np
import gym_pih.hardware.TIS as TIS

class Camera():
    def __init__(self):
        self.tis = TIS.TIS()
        self.tis.open_device("33124075", 1280, 720, "60/1", TIS.SinkFormats.BGRA, False)
        self.tis.set_property("ExposureAuto", "Off")
        self.tis.set_property("GainAuto", "Off")
        self.tis.set_property("ExposureTime", 33333)  # 1/30s
        self.tis.set_property("Gain", 0)
        
        self.latest_image = None
        self.running = False
        self.lock = threading.Lock()
        self.thread = threading.Thread(target=self._update_image)

    def _update_image(self):
        while self.running:
            if self.tis.snap_image(1):
                image = self.tis.get_image()
                image = image[:, :, :3]
                with self.lock:
                    self.latest_image = image

    def start(self):
        self.running = True
        self.tis.start_pipeline()
        self.thread.start()

    def stop(self):
        self.running = False
        self.thread.join()
        self.tis.stop_pipeline()

    def get_latest_image(self):
        with self.lock:
            return self.latest_image.copy() if self.latest_image is not None else None

# Usage example
if __name__ == "__main__":
    camera = Camera()
    camera.start()

    try:
        while True:
            image = camera.get_latest_image()
            if image is not None:
                cv2.imshow("Camera Feed", image)
                if cv2.waitKey(1) & 0xFF == ord('q'):
                    break
    finally:
        camera.stop()
        cv2.destroyAllWindows()
